#pragma once

#ifndef ROBMATH_H
#define ROBMATH_H

#define KIN_PI					       (3.141592653589793238462643383279)
#define KIN_2PI					       (6.283185307179586476925286766559)
#define KIN_PIOVER180                  (0.017453292519943295769236907684886)
#define KIN_180OVERPI                  (57.295779513082320876798154814105)

#define WRIST_SINGULARITY_THRESHOLD    (1e-6)
#define ROBOT_EPS                      (1e-10)



//Order euler_z (rotate about Z axis)->euler_y(rotate about new Y axis)->euler_x(roate about new X axis)
typedef struct {
	double euler_x;
	double euler_y;
	double euler_z;
}RobotEulerZYX;

// roll = euler_x
// pitch = euler_y
// yaw = euler_z
// Order roll->pitch->yaw, rotating about fixed world axis
typedef struct {
	double roll;
	double pitch;
	double yaw;
}RobotRollPitchYaw;


typedef struct {
	double q0;
	double q1;
	double q2;
	double q3;
}RobotQuaternion;


typedef struct {
	double x;
	double y;
	double z;
}RobotPosition;


typedef double RobotRotationMatrix[3][3];
typedef	double RobotTransformationMatrix[4][4];
typedef double RobotMatrix3x3[3][3];
typedef double RobotMatrix4x4[4][4];
typedef double RobotMatrix6x6[6][6];
typedef double RobotTwist[6];
typedef double RobotVector[3];
typedef double RobotVectorSix[6];
typedef double RobotFrame4x4[4][4];

// Basic Vecotr and Matrix Operation
double Norm(const RobotVector v);
void UnitVector(const RobotVector u,RobotVector v);
double Dot(const RobotVector u, const RobotVector v);
void Cross(const RobotVector u, const RobotVector v, RobotVector w);

// EulerZYX, RollPitchYaw, Quaternion, Rotation Matrix Conversion
void EulerZYX2Rot(const RobotEulerZYX* ptr_euler_zyx, RobotRotationMatrix rot);
void Quaternion2Rot(const RobotQuaternion* ptr_quaternion, RobotRotationMatrix rot);
void RollPitchYaw2Rot(const RobotRollPitchYaw* ptr_rpy, RobotRotationMatrix rot);
void Rot2EulerZYX(const RobotRotationMatrix rot, RobotEulerZYX* ptr_euler_zyx);
void Rot2Quaternion(const RobotRotationMatrix rot, RobotQuaternion* ptr_quaternion);
void Rot2RollPitchYaw(const RobotRotationMatrix rot, RobotRollPitchYaw* ptr_rpy);
void Quaternion2EulerZYX(const RobotQuaternion* ptr_quaternion, RobotEulerZYX* ptr_euler_zyx);

// Basic Math Operation Functions for Product of Exponential 
void Hat(const RobotVector w, RobotMatrix3x3 hat_w);
void Expr(const RobotVector w, double theta, RobotRotationMatrix rot);
void Expg(const RobotTwist xi, double theta, RobotTransformationMatrix g);
void Logr(const RobotRotationMatrix rot, RobotVector w, double *ptr_theta);
void Logg(const RobotTransformationMatrix g, RobotTwist xi, double *ptr_theta);
void AdjointMatrix(const RobotTransformationMatrix g, RobotMatrix6x6 adg);
void AdjointInverseMatrix(const RobotTransformationMatrix g, RobotMatrix6x6 adig);
void AdjointTwist(const  RobotTwist xi, RobotMatrix6x6 adx);
void QInverse(const RobotQuaternion* ptr_q, RobotQuaternion* ptr_inv_q);
void QLog(const RobotQuaternion* ptr_q, RobotQuaternion* ptr_log_q);
void QMultiply(const RobotQuaternion* ptr_p, const RobotQuaternion* ptr_q, RobotQuaternion* ptr_pxq);
void QFrameMultiply(const RobotMatrix4x4 matrix1,const RobotMatrix4x4 matrix2,RobotMatrix4x4 result);

//
void ConvertVectorToFrame(const bool euler_or_rpy,const RobotVectorSix vector,RobotFrame4x4 frame);
void ConvertFrameToVector(const bool euler_or_rpy,const RobotFrame4x4 frame,RobotVectorSix vector);
void FrameInversion(const RobotFrame4x4 frame,RobotFrame4x4 inv_frame);

#endif // ROBMATH_H
